import math

def p2tf_val(x, y, z):
    print('正在转换为机械臂坐标')
    # kwsk = kawasaki
    kwsk_point = []
    for i in range(1, len(x)):


        AE = 10  #A是目标点，E是机械臂位置

        #目标点切线与xy平面的夹角
        tan_z_xy = (z[i] - z[i-1] )/( ((x[i] - x[i-1]) ** 2 + (y[i] - y[i-1]) ** 2) ** 0.5);

        #目标点切线在xy平面的投影与x轴的夹角
        tan_x_y= (y[i] - y[i-1]) /(x[i] - x[i-1])


        # 求角度
        arc_z_xy = math.atan(tan_z_xy)
        arc_x_y =math.atan(tan_x_y)

        AE_z = AE*math.cos(arc_z_xy);  #AE在z轴上的投影
        AE_xy=AE*math.sin(arc_z_xy);  #AE在xy平面上的投影

        AE_x=AE_xy*math.cos(arc_x_y);
        AE_y= AE_xy*math.sin(arc_x_y);

        E_x = x[i]+AE_x;
        E_y = y[i]+AE_y;
        E_z = z[i]+AE_z;

        E = [round(E_x, 3), round(E_y, 3), round(E_z, 3), math.degrees(arc_z_xy), math.degrees(arc_x_y), 0]

        kwsk_point.append(E)


    return kwsk_point